package fem2.element;

import fem2.Debugger;
import fem2.MaterialModel;
import fem2.MeshPart;
import fem2.enu.EchoLevelType;
import inf.jlinalg.Array1DMatrix;
import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.BLAM;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IMatrixReadOnly;
import inf.jlinalg.IVector;
import inf.jlinalg.IVectorReadOnly;
import inf.math.LinAlg;

import java.util.ArrayList;

import math2.LinAlgUtilities;
import math2.MathUtilities;

/**
 * original constant crack element based on Linder-Armero formulation.
 * Subsequently, it will be replaced by SDA element and respective derivation
 * 
 * @author hbui
 * 
 */
public class ConstantCrackElement extends EmbeddedCrackElement {

	protected double H, km;

	protected ArrayVector renh;
	private IVector Xi;

	private IMatrix Kdxi;
	private IMatrix Kxid;
	private Array1DMatrix Kxixi_1;
	private Array1DMatrix Kxixi;

	private boolean isFullyOpen = false;
	private boolean isFullyOpenOld = false;

	public ConstantCrackElement(MeshPart mp, MaterialModel mm, double H, double km, double ft) {
		super(mp, mm, ft);
		this.H = H;
		this.km = km;
	}

	@Override
	public void Initialize() {
		super.Initialize();
	}

	@Override
	public void InitializeSolutionStep() {
		super.InitializeSolutionStep();
		// /*
		// * update renh
		// */
		// if (isCrack()) {
		// updateRenh();
		// }
	}

	@Override
	public void InitalizeNonlinearIteration() {
	}

	@Override
	public void FinalizeNonlinearIteration() {
		MeshPart mp = getMeshPart();
		if (!isCrack()) {
			super.FinalizeNonlinearIteration();
		} else {
			int n = mp.countDofs();
			int dim = mp.getDimension();
			if (getEchoLevel().contain(EchoLevelType.AUX4)) {
				Debugger.watch("renh = ", renh);
			}
			/*
			 * update crack opening parameter
			 */
			ArrayVector due = new ArrayVector(mp.getUHatLocal().clone());
			LinAlg.add(n, -1.0, uHatLocal, 1.0, due.getValues());
			ArrayVector dxi = new ArrayVector(dim);
			BLAM.multiply(-1.0, BLAM.NO_TRANSPOSE, Kxid, due, 0.0, dxi);
			BLAM.add(1.0, renh, dxi);
			Array1DMatrix Kxixi_ = (Array1DMatrix) Kxixi.clone();
			LinAlgUtilities.backSolve(Kxixi_, dxi.getValues());
			BLAM.add(1.0, dxi, Xi);

			if (getEchoLevel().contain(EchoLevelType.AUX4)) {
				Debugger.watch("Xi = ", Xi);
			}

			/*
			 * material update (stress, tangent operator)
			 */
			super.FinalizeNonlinearIteration();

			/*
			 * update local equilibrium
			 */
			this.updateLocalEquilibrium();

			if (getEchoLevel().contain(EchoLevelType.AUX4)) {
				Debugger.watch("ue = ", mp.getUHatLocal());
				Debugger.watch("ue_old = ", uHatLocal);
				Debugger.watch("due = ", due);
				Debugger.watch("dxi = ", dxi);
			}
		}
		/*
		 * save ue
		 */
		LinAlg.copy(uHatLocal.length, mp.getUHatLocal(), uHatLocal);

	}

	@Override
	public void FinalizeSolutionStep() {
		/*
		 * check limit state and compute crack
		 */
		this.calculateLimitState();
		/*
		 * crack post-process
		 */
		if (isCrack()) {
			/*
			 * print out results after step complete
			 */
			if (getEchoLevel().contain(EchoLevelType.OUTPUT)) {
				Debugger.watch("crack equilibrium: ", renh);
				Debugger.watch("crack parameter (Xi): ", Xi);
			}
			/*
			 * check equilibrium condition
			 */
			double norm_renh = BLAM.norm(BLAM.NORM_TWO, renh);
			if (norm_renh > MathUtilities.tol6) {
				Debugger.error("crack equilibrium condition is not met at " + getElemType() + " "
						+ getMeshPart().getId() + ", norm(renh) = " + norm_renh);
			}
		}
	}

	@Override
	public void CalculateOutputData() {
	}

	@Override
	public IMatrixReadOnly computeKte(IMatrix ke) {
		ke = (IMatrix) super.computeKte(ke);

		if (isCrack()) {
			IMatrix Kxid_ = (IMatrix) Kxid.clone();
			Array1DMatrix Kxixi_ = (Array1DMatrix) Kxixi.clone();
			LinAlgUtilities.backSolve(Kxixi_, Kxid_);
			BLAM.multiply(-1.0, BLAM.NO_TRANSPOSE, Kdxi, BLAM.NO_TRANSPOSE, Kxid_, 1.0, ke);
			if (getEchoLevel().contain(EchoLevelType.AUX3)) {
				Debugger.watch("Kxixi = \n", Kxixi);
				Debugger.watch("Kxid = \n", Kxid);
				Debugger.watch("(Kxii)^(-1) Kxid = \n", Kxid_);
				Debugger.watch("Kt (crack) = \n", ke);
			}
		}

		if (getEchoLevel().contain(EchoLevelType.AUX2)) {
			Debugger.watch("ke = ", ke);
		}

		return ke;
	}

	@Override
	public IVectorReadOnly computeRie(IVector rie) {
		rie = (IVector) super.computeRie(rie);

		if (isCrack()) {

			if (getEchoLevel().contain(EchoLevelType.AUX4)) {
				Debugger.watch("renh = ", renh);
			}

			ArrayVector renh_ = (ArrayVector) renh.clone();
			Array1DMatrix Kxixi_ = (Array1DMatrix) Kxixi.clone();
			LinAlgUtilities.backSolve(Kxixi_, renh_.getValues());
			BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, Kdxi, renh_, 1.0, rie);
		}

		if (getEchoLevel().contain(EchoLevelType.AUX2)) {
			Debugger.watch("rie = ", rie);
		}

		return rie;
	}

	/**
	 * compute enhanced strain operator from matrix B
	 * 
	 * @param B
	 * @return
	 */
	protected IMatrix computeEnhancedStrainOperator(IMatrix B) {
		MeshPart mp = getMeshPart();
		int dim = mp.getDimension();
		int ntrs = dim * (dim + 1) / 2;
		IMatrix Ba = new Array2DMatrix(ntrs, dim);
		IMatrix Gc = new Array2DMatrix(ntrs, dim);
		ArrayList<Integer> posDomain = getPositiveDomain();
		for (int i = 0; i < posDomain.size(); i++) {
			int shift = dim * (posDomain.get(i));
			Ba.add(0, 0, -B.get(0, shift));
			Ba.add(1, 1, -B.get(1, 1 + shift));
			Ba.add(2, 0, -B.get(2, shift));
			Ba.add(2, 1, -B.get(2, 1 + shift));
		}
		IMatrix J = computeCrackInterpolation(0);
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, Ba, BLAM.NO_TRANSPOSE, J, 0.0, Gc);
		return Gc;
	}

	@Override
	protected IMatrix computeEnhancedStrainOperator(double... xi) {
		MeshPart mp = getMeshPart();
		int dim = mp.getDimension();
		int nn = mp.countModes();
		int ntrs = dim * (dim + 1) / 2;
		Array1DMatrix B = Array1DMatrix.createColumnMajor(ntrs, dim * nn);
		computeB(mp, B, xi);
		return computeEnhancedStrainOperator(B);
	}

	@Override
	protected IMatrix computeProjectionOperator(double... xi) {
		MeshPart mp = getMeshPart();
		int dim = mp.getDimension();
		int ntrs = dim * (dim + 1) / 2;
		IMatrix Ge = new Array2DMatrix(ntrs, dim);
		double coeff = -crackLength / area;
		Ge.set(0, 0, coeff * (normal[0] * normal[0]));
		Ge.set(1, 0, coeff * (normal[1] * normal[1]));
		Ge.set(2, 0, coeff * (2 * normal[0] * normal[1]));
		Ge.set(0, 1, coeff * (normal[0] * tangent[0]));
		Ge.set(1, 1, coeff * (normal[1] * tangent[1]));
		Ge.set(2, 1, coeff * (normal[0] * tangent[1] + normal[1] * tangent[0]));
		return Ge;
	}

	@Override
	public IVector getCrackOpeningParameter() {
		return Xi;
	}

	@Override
	public IVector computeStrain(double... xi) {
		if (!isCrack()) {
			return super.computeRegularStrain(xi);
		} else {
			IVector e = super.computeRegularStrain(xi);
			IMatrix Gc = computeEnhancedStrainOperator(xi);
			BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, Gc, Xi, 1.0, e);
			return e;
		}
	}

	/**
	 * update local equilibrium on crack surface
	 */
	protected void updateLocalEquilibrium() {
		MeshPart mp = getMeshPart();
		int dim = mp.getDimension();

		/*
		 * compute TGamma
		 */
		IMatrix J = computeCrackInterpolation(0);
		IVector u_mu = new ArrayVector(dim);
		IVector n = new ArrayVector(normal);
		IVector m = new ArrayVector(tangent);
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, J, Xi, 0.0, u_mu);
		double u_mu_n = BLAM.dot(u_mu, n);
		double u_mu_m = BLAM.dot(u_mu, m);
		double T = ft + H * Math.abs(u_mu_n);
		double Tn = Math.max(T, 0);
		double Tm = km * u_mu_m;
		IVector TGamma = (IVector) n.clone();
		BLAM.scale(Tn, TGamma);
		BLAM.add(Tm, m, TGamma);

		if (T < 0) {
			isFullyOpen = true;
		} else {
			isFullyOpen = false;
		}

		/*
		 * compute renh
		 */
		double t = getMaterialModel().getSectionProperty();
		BLAM.multiply(-crackLength * t, BLAM.TRANSPOSE, J, TGamma, 0.0, renh);

		if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
			Debugger.watch("J = ", J);
			Debugger.watch("Xi = ", Xi);
			Debugger.watch("Tgamma = ", TGamma);
		}

		int ng = getNumIntegrationPoints();
		for (int k = 0; k < ng; k++) {
			double[] xi = getIntegrationPoint(k);
			double w = getIntegrationWeight(k);
			double[][] jac = mp.jacXAt(xi);
			double detJac = LinAlgUtilities.determinantNxN(jac);
			IMatrix Ge = computeProjectionOperator(xi);
			IVector sigma = getStress(k);
			BLAM.multiply(-w * detJac * t, BLAM.TRANSPOSE, Ge, sigma, 1.0, renh);

			if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
				Debugger.watch("Ge = ", computeProjectionOperator());
				Debugger.watch("sigma = ", getStress(0));
			}
		}

		if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
			Debugger.watch("renh = ", renh);
		}
		/*
		 * update Kxixi
		 */
		if (isFullyOpen && !isFullyOpenOld) {
			BLAM.copy(Kxixi_1, Kxixi);
			// /*
			// * add in small term to improve convergence property
			// */
			// int r = Kxixi.getRowCount();
			// double kappa = 1e-3;
			// for (int i = 0; i < r; i++) {
			// Kxixi.add(i, i, kappa);
			// }
			isFullyOpenOld = true;
			Debugger.warn("crack is fully open at element " + mp.getId());
		} else if (!isFullyOpen && isFullyOpenOld) {
			IMatrix Ctilde = computeCtilde(Math.signum(u_mu_n));
			BLAM.copy(Kxixi_1, Kxixi);
			LinAlgUtilities.btCB(J, Ctilde, crackLength * t, Kxixi);
			isFullyOpenOld = false;
			Debugger.warn("crack is partially open at element " + mp.getId());
		}
	}

	/**
	 * perform modification at the limit state
	 */
	protected void calculateLimitState() {
		if (isActivated() && !isCrack()) {
			checkCrackCriterion();
			if (isCrack()) {
				MeshPart mp = getMeshPart();
				if (getEchoLevel().contain(EchoLevelType.OUTPUT)) {
					Debugger.warn("crack initited at element " + mp.getId());
				}
				/*
				 * compute Kdxi, Kxid, Kxixi_1
				 */
				int dim = mp.getDimension();
				int nn = mp.countModes();
				int ntrs = dim * (dim + 1) / 2;
				int ng = getNumIntegrationPoints();
				Kdxi = new Array2DMatrix(dim * nn, dim);
				Kxid = new Array2DMatrix(dim, dim * nn);
				Kxixi_1 = Array1DMatrix.createRowMajor(dim, dim);
				Kxixi = Array1DMatrix.createRowMajor(dim, dim);
				Array1DMatrix B = Array1DMatrix.createColumnMajor(ntrs, dim * nn);
				double t = getMaterialModel().getSectionProperty();

				for (int k = 0; k < ng; k++) {
					double[] xi = getIntegrationPoint(k);
					double w = getIntegrationWeight(k);
					double[][] jac = mp.jacXAt(xi);
					double detJac = LinAlgUtilities.determinantNxN(jac);

					computeB(mp, B, xi);
					IMatrix C = getMaterialModel().getConstitutiveMatrix(this, k);

					IMatrix Gc = computeEnhancedStrainOperator(B);
					LinAlgUtilities.atCB(B, Gc, C, w * detJac * t, Kdxi);

					IMatrix Ge = computeProjectionOperator(xi);
					LinAlgUtilities.atCB(Ge, B, C, w * detJac * t, Kxid);

					LinAlgUtilities.atCB(Ge, Gc, C, w * detJac * t, Kxixi_1);

					if (getEchoLevel().contain(EchoLevelType.AUX3)) {
						Debugger.watch("Gc =", Gc);
					}
				}

				/*
				 * compute Kxixi
				 */
				IMatrix J = computeCrackInterpolation(0);
				IMatrix Ctilde = computeCtilde(1.0);
				BLAM.copy(Kxixi_1, Kxixi);
				LinAlgUtilities.btCB(J, Ctilde, crackLength * t, Kxixi);

				/*
				 * initialize renh, Xi
				 */
				renh = new ArrayVector(dim);
				Xi = new ArrayVector(dim);
				isFullyOpen = false;
				isFullyOpenOld = false;

				if (getEchoLevel().contain(EchoLevelType.AUX3)) {
					Debugger.watch("Kdxi = ", Kdxi);
					Debugger.watch("Kxid = ", Kxid);
					Debugger.watch("Kxixi = ", Kxixi);
					Debugger.watch("Kxixi_1 = ", Kxixi_1);
					// Debugger.watch("renh = ", renh);
				}
			}
		}
	}

	@Override
	protected IVector getRepresentativeStress() {
		IVector sigmaAvg = (IVector) getStress(0).clone();
		int ng = getNumIntegrationPoints();
		for (int k = 1; k < ng; k++) {
			BLAM.add(1.0, getStress(k), sigmaAvg);
		}
		BLAM.scale(1.0 / ng, sigmaAvg);
		return sigmaAvg;
	}

	/**
	 * compute J operator
	 * 
	 * @param s
	 *            local coordinate on the crack
	 * @return J operator
	 */
	protected IMatrix computeCrackInterpolation(double s) {
		int dim = getMeshPart().getDimension();
		IMatrix J = new Array2DMatrix(dim, dim);
		J.set(0, 0, normal[0]);
		J.set(1, 0, normal[1]);
		J.set(0, 1, tangent[0]);
		J.set(1, 1, tangent[1]);
		return J;
	}

	/**
	 * compute Ctilde matrix
	 * 
	 * @param sign_u_mu_n
	 *            sign of u_mu_n
	 * @return
	 */
	protected IMatrix computeCtilde(double sign_u_mu_n) {
		int dim = getMeshPart().getDimension();
		IMatrix Ctilde = new Array2DMatrix(dim, dim);
		ArrayVector n = new ArrayVector(normal);
		ArrayVector m = new ArrayVector(tangent);
		BLAM.multiply(sign_u_mu_n * H, n, n, Ctilde);
		BLAM.multiply(km, m, m, Ctilde);
		return Ctilde;
	}
}
